#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>

#include "math.h"

int fd = 0;
			
typedef struct{
    float AX;
    float AY;
    float AZ;
    float GX;
    float GY;
    float GZ;
}param_imu;

typedef struct{
    float Pitch;
    float Roll;
    float Yaw;
}param_Angle;

extern param_Angle imu_Angle;
							
							
void IMU_getEuleranAngles(void);

#define kp 				20.00f
#define ki 				0.001f
#define cycle_T 		0.005f//200hz
#define half_T 			0.0025f

int16_t Ax,Ay,Az,Gx,Gy,Gz;
param_imu imu_data;
param_Angle imu_Angle;

float q[4] = {1.0,0.0,0.0,0.0};
float exInt = 0.0 ,eyInt = 0.0 ,ezInt = 0.0 ;

//////////////////////////////////////////////////////////////////////////////////
float fast_sqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *) &y;
    i = 0x5f3759df - (i >> 1);  //0x5f3759df是一个平方根倒数速算法
    y = *(float *) &i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}
//////////////////////////////////////////////////////////////////////////////////

void IMU_GetValues(void)
{
    short value[6];
    read(fd,value,sizeof(value));
    Ax=value[0];Ay=value[1];Az=value[2];
    Gx=value[3];Gy=value[4];Gz=value[5];

    imu_data.AX = ((float)Ax)/2048;
    imu_data.AY = ((float)Ay)/2048;
    imu_data.AZ = ((float)Az)/2048;

    imu_data.GX = ((float)Gx)*0.001064;
    imu_data.GY = ((float)Gy)*0.001064;
    imu_data.GZ = ((float)Gz)*0.001064;
}
//一阶互补滤波，参数为：1、加速度计得出的角度  2、角速度
float First_order_filter_x(float angle_m, float gyro_m)
{
   static float angle;
   float K1 =0.02;
   angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * cycle_T);
   return angle;
}
void IMU_AHRSupdate(param_imu* imu_temp)
{
	float ax,ay,az;
	float gx,gy,gz;
	ax = imu_temp->AX;
	ay = imu_temp->AY;
	az = imu_temp->AZ;
	gx = imu_temp->GX;
	gy = imu_temp->GY;
	gz = imu_temp->GZ;
	
	float vx, vy, vz; 
	float ex, ey, ez; 
	
	float q0 = q[0];
    float q1 = q[1];
    float q2 = q[2];
    float q3 = q[3];
	
    //实际测量到的重力加速度G=(ax,ay,az)
	float norm = fast_sqrt(ax*ax+ay*ay+az*az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
	//重力加速度是向量，所以当它俩夹角相同的时候，可以认为没有误差,在这里只看夹角误差
	//角速度所在的坐标系得出的四元数，利用角速度的四元数得出地球系重力加速度在原坐标系应该出现的向量坐标G=(vx,vy,vz)\
     将n=[0,0,1]带入四元数的转置矩阵==取转置矩阵xyz的z分量列,看不懂理解不了那就是算法所需，这就是算法，没那么多为什么
    vx = 2 * (q1*q3 - q0*q2);
	vy = 2 * (q2*q3 + q0*q1);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
	
    //(vx,vy,vz)外积(ax,ay,az)
    ex = (ay * vz - az * vy);//  |A|*|B|*sin<A,B> 误差可表示为:理论的加速度与实际的加速度做外积，|a|*|b|*sin(x)
    ey = (az * vx - ax * vz);//进行了归一化仅剩角度，而角度差距较小的情况下sin可当作正比,向量的外积其实可以近似认为是角度。
    ez = (ax * vy - ay * vx);
	
	exInt += ex;
	eyInt += ey;
	ezInt += ez;
	
    //我发现pid算法怎么应用其实很固定的，普通人真想不到怎么用，在某些地方不靠参考根本就不会，大部分都只是自己调参数
	gx += kp * ex + ki*exInt;
	gy += kp * ey + ki*eyInt;
	gz += kp * ez + ki*ezInt;

    //公式:根据角速度得到四元数,如下同理
    //一阶龙格库塔法更新四元数
    //q0(t + Δt) = q0(t) + (-q1(t) * gx - q2(t) * gy - q3(t) * gz) * Δt / 2
	q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_T;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy)  * half_T;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx)  * half_T;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx)  * half_T;
	
	norm = fast_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	
    //fast_sqrt得到的是平方根开完后的倒数，单位化直接乘即可,这也叫归一化
	q[0] = q0 * norm;
	q[1] = q1 * norm;
	q[2] = q2 * norm;
	q[3] = q3 * norm;
	
}

void IMU_getEuleranAngles(void)
{
	IMU_GetValues();
	IMU_AHRSupdate(&imu_data);
	
    //公式：根据四元数得到欧拉角------z->y->x
	imu_Angle.Pitch = asin(-2 * q[1] * q[3] + 2 * q[0]* q[2])* 57.2957;//乘以 57.2957 是为了将计算得到的俯仰角从弧度转换为度
	imu_Angle.Roll  = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2]* q[2] + 1)* 57.2957;
	imu_Angle.Yaw  += imu_data.GZ* 57.2957* cycle_T* 4;
	
}   






int main(int argc ,char *argv[])
{
    static int i;
    fd = open(argv[1],O_RDWR);
    while(1)
    {
        IMU_getEuleranAngles();
        printf("x:%d\ty:%d\tz:%d\tcount:i=%d\r\n",(int)imu_Angle.Roll,(int)imu_Angle.Pitch,(int)imu_Angle.Yaw,i++);
        usleep(10*1000);
    }
    close(fd);
    return 0;
}
/*arm-linux-gnueabihf-gcc -march=armv7-a -mfpu=neon -mfloat-abi=hard -lm mpu6050APP.c -o mpu6050APP*/